//
// Created by pulsarv on 2021/2/23.
//

#include <rc_controller/pid_controller.h>
#include <iostream>

namespace follow_down {
    namespace move {

        pid_controller::pid_controller(float p, float i, float d) :
                kp(p),
                ki(i),
                kd(d),
                e_pre_1(0),
                e_pre_2(0),
                target(0),
                actual(0) {
            A = kp + ki + kd;
            B = -2 * kd - kp;
            C = kd;
            e = target - actual;
        }

        pid_controller::pid_controller() : kp(0), ki(0), kd(0), e_pre_1(0), e_pre_2(0), target(0), actual(0) {
            A = kp + ki + kd;
            B = -2 * kd - kp;
            C = kd;
            e = target - actual;
        }

        float pid_controller::pid_control(float tar, float act) {
            float u_increment;
            target = tar;
            actual = act;
            e = target - actual;
            u_increment = A * e + B * e_pre_1 + C * e_pre_2;
            e_pre_2 = e_pre_1;
            e_pre_1 = e;
            return u_increment;
        }

        void pid_controller::pid_show() {
            std::cout << "The infomation of this incremental PID controller is as following:"
                      << std::endl;
            std::cout << "     Kp=" << kp << std::endl;
            std::cout << "     Ki=" << ki << std::endl;
            std::cout << "     Kd=" << kd << std::endl;
            std::cout << " target=" << target << std::endl;
            std::cout << " actual=" << actual << std::endl;
            std::cout << "      e=" << e << std::endl;
            std::cout << "e_pre_1=" << e_pre_1 << std::endl;
            std::cout << "e_pre_2=" << e_pre_2 << std::endl;
        }
    } // namespace move
} // namespace follow_down
